/*
* Copyright (c) 2006-2007 Erin Catto http:
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked, and must not be
* misrepresented the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

goog.provide('box2d.GearJoint');

goog.require('box2d.GearJointDef');
goog.require('box2d.Joint');
/**
 @constructor
 @extends {box2d.Joint}
 @param {!box2d.GearJointDef} def
 */
box2d.GearJoint = function(def) {
  // The constructor for b2Joint
  // initialize instance variables for references
  this.m_node1 = new box2d.JointNode();
  this.m_node2 = new box2d.JointNode();
  //
  this.m_type = def.type;
  this.m_prev = null;
  this.m_next = null;
  this.m_body1 = def.body1;
  this.m_body2 = def.body2;
  this.m_collideConnected = def.getCollideConnected();
  this.m_islandFlag = false;
  this.m_userData = def.userData;
  //
  // initialize instance variables for references
  this.m_groundAnchor1 = new box2d.Vec2();
  this.m_groundAnchor2 = new box2d.Vec2();
  this.m_localAnchor1 = new box2d.Vec2();
  this.m_localAnchor2 = new box2d.Vec2();
  this.m_J = new box2d.Jacobian();
  //
  // parent constructor
  //super(def);
  //box2d.Settings.b2Assert(def.joint1.m_type == box2d.Joint.e_revoluteJoint || def.joint1.m_type == box2d.Joint.e_prismaticJoint);
  //box2d.Settings.b2Assert(def.joint2.m_type == box2d.Joint.e_revoluteJoint || def.joint2.m_type == box2d.Joint.e_prismaticJoint);
  //box2d.Settings.b2Assert(def.joint1.m_body1.IsStatic());
  //box2d.Settings.b2Assert(def.joint2.m_body1.IsStatic());
  this.m_revolute1 = null;
  this.m_prismatic1 = null;
  this.m_revolute2 = null;
  this.m_prismatic2 = null;

  var coordinate1;
  var coordinate2;

  this.m_ground1 = def.joint1.m_body1;
  this.m_body1 = def.joint1.m_body2;
  if (def.joint1.m_type == box2d.Joint.e_revoluteJoint) {
    this.m_revolute1 = def.joint1;
    this.m_groundAnchor1.SetV(this.m_revolute1.m_localAnchor1);
    this.m_localAnchor1.SetV(this.m_revolute1.m_localAnchor2);
    coordinate1 = this.m_revolute1.GetJointAngle();
  } else {
    this.m_prismatic1 = def.joint1;
    this.m_groundAnchor1.SetV(this.m_prismatic1.m_localAnchor1);
    this.m_localAnchor1.SetV(this.m_prismatic1.m_localAnchor2);
    coordinate1 = this.m_prismatic1.GetJointTranslation();
  }

  this.m_ground2 = def.joint2.m_body1;
  this.m_body2 = def.joint2.m_body2;
  if (def.joint2.m_type == box2d.Joint.e_revoluteJoint) {
    this.m_revolute2 = def.joint2;
    this.m_groundAnchor2.SetV(this.m_revolute2.m_localAnchor1);
    this.m_localAnchor2.SetV(this.m_revolute2.m_localAnchor2);
    coordinate2 = this.m_revolute2.GetJointAngle();
  } else {
    this.m_prismatic2 = def.joint2;
    this.m_groundAnchor2.SetV(this.m_prismatic2.m_localAnchor1);
    this.m_localAnchor2.SetV(this.m_prismatic2.m_localAnchor2);
    coordinate2 = this.m_prismatic2.GetJointTranslation();
  }

  this.m_ratio = def.ratio;

  this.m_constant = coordinate1 + this.m_ratio * coordinate2;

  this.m_impulse = 0.0;

};
goog.inherits(box2d.GearJoint, box2d.Joint);

box2d.GearJoint.prototype.GetAnchor1 = function() {
  //return this.m_body1.m_position + b2MulMV(this.m_body1.m_R, this.m_localAnchor1);
  var tMat = this.m_body1.m_R;
  return new box2d.Vec2(this.m_body1.m_position.x + (tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y), this.m_body1.m_position.y + (tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y));
};

box2d.GearJoint.prototype.GetAnchor2 = function() {
  //return this.m_body2->m_position + b2Mul(this.m_body2->m_R, this.m_localAnchor2);
  var tMat = this.m_body2.m_R;
  return new box2d.Vec2(this.m_body2.m_position.x + (tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y), this.m_body2.m_position.y + (tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y));
};

box2d.GearJoint.prototype.GetReactionForce = function(invTimeStep) {
  //box2d.Vec2 F(0.0f, 0.0f);
  return new box2d.Vec2();
};

box2d.GearJoint.prototype.GetReactionTorque = function(invTimeStep) {
  return 0.0;
};

box2d.GearJoint.prototype.GetRatio = function() {
  return this.m_ratio;
};

//--------------- Internals Below -------------------
box2d.GearJoint.prototype.PrepareVelocitySolver = function() {
  var g1 = this.m_ground1;
  var g2 = this.m_ground2;
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  // temp vars
  var ugX;
  var ugY;
  var rX;
  var rY;
  var tMat;
  var tVec;
  var crug;

  var K = 0.0;
  this.m_J.SetZero();

  if (this.m_revolute1) {
    this.m_J.angular1 = -1.0;
    K += b1.m_invI;
  } else {
    //box2d.Vec2 ug = b2MulMV(g1->m_R, this.m_prismatic1->m_localXAxis1);
    tMat = g1.m_R;
    tVec = this.m_prismatic1.m_localXAxis1;
    ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
    ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
    //box2d.Vec2 r = b2MulMV(b1->m_R, this.m_localAnchor1);
    tMat = b1.m_R;
    rX = tMat.col1.x * this.m_localAnchor1.x + tMat.col2.x * this.m_localAnchor1.y;
    rY = tMat.col1.y * this.m_localAnchor1.x + tMat.col2.y * this.m_localAnchor1.y;

    //var crug = b2Cross(r, ug);
    crug = rX * ugY - rY * ugX;
    //this.m_J.linear1 = -ug;
    this.m_J.linear1.Set(-ugX, -ugY);
    this.m_J.angular1 = -crug;
    K += b1.m_invMass + b1.m_invI * crug * crug;
  }

  if (this.m_revolute2) {
    this.m_J.angular2 = -this.m_ratio;
    K += this.m_ratio * this.m_ratio * b2.m_invI;
  } else {
    //box2d.Vec2 ug = b2Mul(g2->m_R, this.m_prismatic2->m_localXAxis1);
    tMat = g2.m_R;
    tVec = this.m_prismatic2.m_localXAxis1;
    ugX = tMat.col1.x * tVec.x + tMat.col2.x * tVec.y;
    ugY = tMat.col1.y * tVec.x + tMat.col2.y * tVec.y;
    //box2d.Vec2 r = b2Mul(b2->m_R, this.m_localAnchor2);
    tMat = b2.m_R;
    rX = tMat.col1.x * this.m_localAnchor2.x + tMat.col2.x * this.m_localAnchor2.y;
    rY = tMat.col1.y * this.m_localAnchor2.x + tMat.col2.y * this.m_localAnchor2.y;
    //float32 crug = b2Cross(r, ug);
    crug = rX * ugY - rY * ugX;
    //this.m_J.linear2 = -this.m_ratio * ug;
    this.m_J.linear2.Set(-this.m_ratio * ugX, -this.m_ratio * ugY);
    this.m_J.angular2 = -this.m_ratio * crug;
    K += this.m_ratio * this.m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
  }

  // Compute effective mass.
  //box2d.Settings.b2Assert(K > 0.0);
  this.m_mass = 1.0 / K;

  // Warm starting.
  //b1.m_linearVelocity += b1.m_invMass * this.m_impulse * this.m_J.linear1;
  b1.m_linearVelocity.x += b1.m_invMass * this.m_impulse * this.m_J.linear1.x;
  b1.m_linearVelocity.y += b1.m_invMass * this.m_impulse * this.m_J.linear1.y;
  b1.m_angularVelocity += b1.m_invI * this.m_impulse * this.m_J.angular1;
  //b2.m_linearVelocity += b2.m_invMass * this.m_impulse * this.m_J.linear2;
  b2.m_linearVelocity.x += b2.m_invMass * this.m_impulse * this.m_J.linear2.x;
  b2.m_linearVelocity.y += b2.m_invMass * this.m_impulse * this.m_J.linear2.y;
  b2.m_angularVelocity += b2.m_invI * this.m_impulse * this.m_J.angular2;
};

box2d.GearJoint.prototype.SolveVelocityConstraints = function(step) {
  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var Cdot = this.m_J.Compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);

  var impulse = -this.m_mass * Cdot;
  this.m_impulse += impulse;

  b1.m_linearVelocity.x += b1.m_invMass * impulse * this.m_J.linear1.x;
  b1.m_linearVelocity.y += b1.m_invMass * impulse * this.m_J.linear1.y;
  b1.m_angularVelocity += b1.m_invI * impulse * this.m_J.angular1;
  b2.m_linearVelocity.x += b2.m_invMass * impulse * this.m_J.linear2.x;
  b2.m_linearVelocity.y += b2.m_invMass * impulse * this.m_J.linear2.y;
  b2.m_angularVelocity += b2.m_invI * impulse * this.m_J.angular2;
};

box2d.GearJoint.prototype.SolvePositionConstraints = function() {
  var linearError = 0.0;

  var b1 = this.m_body1;
  var b2 = this.m_body2;

  var coordinate1;
  var coordinate2;
  if (this.m_revolute1) {
    coordinate1 = this.m_revolute1.GetJointAngle();
  } else {
    coordinate1 = this.m_prismatic1.GetJointTranslation();
  }

  if (this.m_revolute2) {
    coordinate2 = this.m_revolute2.GetJointAngle();
  } else {
    coordinate2 = this.m_prismatic2.GetJointTranslation();
  }

  var C = this.m_constant - (coordinate1 + this.m_ratio * coordinate2);

  var impulse = -this.m_mass * C;

  b1.m_position.x += b1.m_invMass * impulse * this.m_J.linear1.x;
  b1.m_position.y += b1.m_invMass * impulse * this.m_J.linear1.y;
  b1.m_rotation += b1.m_invI * impulse * this.m_J.angular1;
  b2.m_position.x += b2.m_invMass * impulse * this.m_J.linear2.x;
  b2.m_position.y += b2.m_invMass * impulse * this.m_J.linear2.y;
  b2.m_rotation += b2.m_invI * impulse * this.m_J.angular2;
  b1.m_R.Set(b1.m_rotation);
  b2.m_R.Set(b2.m_rotation);

  return linearError < box2d.Settings.b2_linearSlop;
};
